/*
 *  acpFilter.h
 *  macx_aTemperature
 *
 *  Created by Acroname on 7/27/09.
 *  Copyright 2009 __MyCompanyName__. All rights reserved.
 *
 */

#ifndef _acpFilter_H_
#define _acpFilter_H_

#include "aIO.h"

/////////////////////////////////////////////////////////////////////

#define aFILTER_MAXSAMPLES 50
#define aFILTER_MOVINGAVGSAMPLES 8


/////////////////////////////////////////////////////////////////////
// Base filter class

class acpFilter {
public:
  acpFilter() {};
  virtual ~acpFilter() {};
  virtual void init(void) = 0;
  virtual float update(const float input) = 0;
  
};

/////////////////////////////////////////////////////////////////////

class acpMovingAverageFilter : public acpFilter {
public:
  acpMovingAverageFilter() : 
    m_index(0), m_nSamples(aFILTER_MOVINGAVGSAMPLES) {};
  void init(void) 
    {for(int i = 0; i< m_nSamples; i++) m_fValues[i] = 0.0f;};
  void setNumSamples(const int n) { m_nSamples = n; init(); };
  float update(const float input);
private:
  int m_nSamples;
  float m_fValues[aFILTER_MAXSAMPLES];  
  int m_index;
};

/////////////////////////////////////////////////////////////////////

class acpKalmanFilter : public acpFilter{
public:
  acpKalmanFilter(void) {};
  ~acpKalmanFilter(void) {};
  void init(void);
  void init(const float a, const float b, const float h);
  float update(const float input);
  
  
private:
  float m_x; // previous state 
  float m_u; // input
  float m_z; // output state
  float m_a; // gain (system gain)
  float m_b; // gain, often zero
  float m_h; // gain (output gain)
  float m_k; // gain
  float m_w; // process noise
  float m_v; // measurement noise
  float m_Q;
  float m_R;
  
  float m_xaposteriori; // state posteriori estimate
  float m_paposteriori; // covariance posteriori
  float m_xapriori; // x before prediction
  float m_residual;
  float m_papriori;
  
};


#endif

